// ---------------------------------------------------------------------------
//  File:geo_spatial.cpp
//
//  Description: Contains geodesy related functions
//
//  Author: golden.colorado.5280@gmail.com
//
//  Date: December 26th, 2015
//
//  Geospatial Algorithms Adapted From:
//      This application uses geodesy algorithms from the open source library
//      geostarslib-0.9.3. This software is available at the following URL.
//      http://sourceforge.net/projects/geostarslib/files/geoStarLib-0.9.3/
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
//  THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
//  DEALINGS IN THE SOFTWARE.
// ---------------------------------------------------------------------------

#include "geo_spatial.h"

//Define the World Geodetic System (WGS) Ellipsoid
// WGS 84 is an Earth-centered, Earth-fixed terrestrial reference system
// and geodetic datum. WGS 84 is based on a consistent set of constants
// and model parameters that describe the Earth's size, shape, and gravity
// and geomagnetic fields.
static void define_WGS84_ellipsoid(    double *a,  //Major axis (meters)
                                double *b,  //Minor axis (meters)
                                double *e2, //Eccentricity
                                double *ee2,//Eccentricity prime
                                double *f){ //Flattening
    *a   = WGS84_a;  //Major Axis
    *f   = GEO_FL(WGS84_fl); //Flattening
    *b   = GEO_B(WGS84_a,WGS84_fl); //Minor Axis
    *e2  = GEO_E2(WGS84_a,WGS84_fl); //Eccentricity squared
    *ee2 = GEO_E2P(WGS84_a,WGS84_fl); //Eccentricity squared prime
}

//Initialize a new site using WGS84 Ellipsoid
//Reference:http://geostarslib.sourceforge.net/
void define_observer(GEO_LOCATION *l, double lat, double lon, double hgt){
    double N,a,e2,ee2, b, flat,Nh;

    //Set datum values for WGS-84 ellipsoid
    define_WGS84_ellipsoid(&a,&b,&e2,&ee2,&flat);
    l->datum.a    = a;          //Major Axis
    l->datum.b    = b;          //Minor Axis
    l->datum.e2   = e2;         //Eccentricity squared
    l->datum.flat = flat;       //Earth Flattening
    l->datum.ee2  = ee2;        //Eccentricity squared prime
    l->datum.m1e2 = 1.0 - e2;

    //Initialize lat/lon values
    if(lon>180.0)lon=lon-360.0; // normalize to -180 to +180
    l->lat = lat;
    l->lon = lon;
    l->hgt = hgt;

    //Convert to radians
    l->rlat = lat * DEG_TO_RAD;
    l->rlon = lon * DEG_TO_RAD;

    //Precompute sin/cos values
    l->slat = sin(l->rlat);
    l->slon = sin(l->rlon);
    l->clat = cos(l->rlat);
    l->clon = cos(l->rlon);
    l->tlat = tan(l->rlat);
    l->clonclat = l->clon * l->clat;
    l->slonslat = l->slon * l->slat;
    l->clonslat = l->clon * l->slat;
    l->slonclat = l->slon * l->clat;

    //Compute the radius of curvature */
    N = a / (sqrt(1.0 - e2 * pow(l->slat,2.0)));

    //Compute the EFG(XYZ) coordinates (earth centered) */
    Nh = N + hgt;
    l->e = Nh * l->clonclat;
    l->f = Nh * l->slonclat;
    l->g = (N * (l->datum.m1e2) + hgt) * l->slat;
    l->efg[GEO_E] = l->e;
    l->efg[GEO_F] = l->f;
    l->efg[GEO_G] = l->g;
}

//Converts from Range, Azimuth, and Elevation into Cartesian coordinates X,Y,Z
//Reference:http://geostarslib.sourceforge.net/
static void geoRae2Xyz (double rae_in[],
                 double xyz_out[]){
    double r_cos_e;
    r_cos_e = rae_in[GEO_RNG] * cos(rae_in[GEO_EL]);
    xyz_out[GEO_X] = sin(rae_in[GEO_AZ]) * r_cos_e;
    xyz_out[GEO_Y] = cos(rae_in[GEO_AZ]) * r_cos_e;
    xyz_out[GEO_Z] = rae_in[GEO_RNG] * sin(rae_in[GEO_EL]);
}

//Returns the EFG coordinates that the Range, Azimuth, Elevation points to from the observer
//Reference:http://geostarslib.sourceforge.net/
void geoRae2Efg (GEO_LOCATION *loc,
                 double aer_in[],
                 double efg_out[]){

    double c1, c2, c3;
    double xyz_val[3];

    /* Convert the RAE value to XYZ: */
    geoRae2Xyz(aer_in, xyz_val);

    /* Do the matrix multiplication: */
    c1 = -loc->slon * xyz_val[GEO_X] +
    -loc->clon * loc->slat * xyz_val[GEO_Y] +
    loc->clon * loc->clat * xyz_val[GEO_Z];

    c2 =  loc->clon * xyz_val[GEO_X] +
    -loc->slon * loc->slat * xyz_val[GEO_Y] +
    loc->slon * loc->clat * xyz_val[GEO_Z];

    c3 = loc->clat * xyz_val[GEO_Y] +
    loc->slat * xyz_val[GEO_Z];

    /* Add resultant matrix to local EFG to get remote EFG: */
    efg_out[GEO_E] = c1 + loc->e;
    efg_out[GEO_F] = c2 + loc->f;
    efg_out[GEO_G] = c3 + loc->g;
}

static double sqr(double num){
    return (num * num);
}

//Transforms 3D Cartesian to geodetic coordinates
//Reference: http://geostarslib.sourceforge.net/
void geoEfg2Llh (GEO_LOCATION *obs, double efg[], double *lat, double *lon, double *hgt){
    double a, e2;
    double a2,x2,y2,z2,d2,e4,p,q,r,s,t,u,v,w,k,d,sqrtx2py2,sqrtd2pz2;

    //Use the datum of the observer
    a = obs->datum.a;           //Major Axis
    e2 = obs->datum.e2;         //Eccentricity squared

   // precompute variables
    x2 = sqr(efg[GEO_E]);
    y2 = sqr(efg[GEO_F]);
    z2 = sqr(efg[GEO_G]);
    a2 = sqr(a);
    e4 = sqr(e2);
    sqrtx2py2 = sqrt(x2+y2);

    // main algorithm
    p = (x2 + y2)/ a2;
    q = ((1.0-e2)/ a2)*z2;
    r = (p+q-e4)/6.0;
    s = e4 * (p*q)/(4.0*pow(r,3.0));
    t = pow(1.0+s+sqrt(s*(2.0+s)),1.0/3.0);
    u = r*(1.0+t+(1.0/t));
    v = sqrt(sqr(u)+e4*q);
    w = e2*(u+v-q)/(2.0*v);
    k = sqrt(u+v+sqr(w)) - w;
    d = k*sqrtx2py2/(k+e2);
    d2 = sqr(d);
    sqrtd2pz2 = sqrt(d2+z2);

    *lon = 2 * atan(efg[GEO_F]/(efg[GEO_E] + sqrtx2py2)) * RAD_TO_DEG;

    *lat = 2 * atan(efg[GEO_G]/(d + sqrtd2pz2)) * RAD_TO_DEG;

    *hgt = ((k + e2 - 1.0) / k) * sqrtd2pz2;
}
